#ifndef DIGITA_HPP
#define DIGITA_HPP
#include "digit.hpp"

#include "opencv2/reg/mapaffine.hpp"
#include "opencv2/reg/mapshift.hpp"
#include "opencv2/reg/mapprojec.hpp"
#include "opencv2/reg/mappergradshift.hpp"
#include "opencv2/reg/mappergradeuclid.hpp"
#include "opencv2/reg/mappergradsimilar.hpp"
#include "opencv2/reg/mappergradaffine.hpp"
#include "opencv2/reg/mappergradproj.hpp"
#include "opencv2/reg/mapperpyramid.hpp"

#include "plate.hpp"

class DigitA : public Digit
{
public:
    DigitA(cv::Rect boundingBox0)
        : Digit(boundingBox0)
    {

    }



    static void showDifference(const cv::Mat& image1, const cv::Mat& image2, const char* title)
    {
        cv::Mat img1, img2;
        image1.convertTo(img1, CV_32FC3);
        image2.convertTo(img2, CV_32FC3);
        if(img1.channels() != 1)
            cv::cvtColor(img1, img1, CV_RGB2GRAY);
        if(img2.channels() != 1)
            cv::cvtColor(img2, img2, CV_RGB2GRAY);



        cv::Mat imgDiff;
        img1.copyTo(imgDiff);
        imgDiff -= img2;
        imgDiff /= 2.f;
        imgDiff += 128.f;

        cv::Mat imgSh;
        imgDiff.convertTo(imgSh, CV_8UC3);
        cv::imshow(title, imgSh);
    }


    static cv::Matx33d MapP(const cv::Mat &img1, const cv::Mat &img2)
    {
        // Register
        cv::Ptr<cv::reg::MapperGradProj> mapper = cv::makePtr<cv::reg::MapperGradProj>();
        cv::reg::MapperPyramid mappPyr(mapper);
        cv::Ptr<cv::reg::Map> mapPtr = mappPyr.calculate(img1, img2);

        // Print result
        cv::reg::MapProjec* mapProj = dynamic_cast<cv::reg::MapProjec*>(mapPtr.get());
        mapProj->normalize();

        cv::Matx33d res;
        cv::Mat(mapProj->getProjTr()).copyTo(res);
#if 0
        // Display registration accuracy
        cv::Mat dest;
        mapProj->inverseWarp(img2, dest);

        cv::Mat dest2;
        cv::warpPerspective(img2, dest2, res.inv(), img2.size());

        //        showDifference(dest2, img2, "dest2");

        showDifference(img1, dest, "Image difference: pixel registered");


        cv::Mat img2warp;
        dest2.convertTo(img2warp,CV_8U);
        cv::imshow("img2warp",img2warp);

        std::cout<<"res="<<res<<"\n"<<std::flush;
#endif


        return res;
    }


    static void testProjective(const cv::Mat& img1)
    {
        cv::Mat img2;

        // Warp original image
        cv::Matx<double, 3, 3> projTr(1., 0., 0., 0., 1., 0., 0.0001, 0.0001, 1);
        cv::reg::MapProjec mapTest(projTr);
        mapTest.warp(img1, img2);
        showDifference(img1, img2, "Image difference");

        cv::Matx33d res;

        res=MapP(img1,img2);

        cvDestroyWindow("Image difference");

    }


    static double ScaleToD(const cv::Mat &m8, cv::Mat &m64, double maxL=400)
    {
        double scale=1;
        double scalex=maxL/m8.cols;
        double scaley=maxL/m8.rows;

        if(scale>scalex) scale=scalex;
        if(scale>scaley) scale=scaley;

        // Convert to double, 3 channels
        m8.convertTo(m64, CV_64F);
        if(scale!=1)
            cv::resize(m64,m64,cv::Size(),scale,scale);
        return scale;
    }


    static void tt()
    {
        cv::Mat img1 = cv::imread("/home/u/number/test2/25[25cmx20cm].png", CV_LOAD_IMAGE_UNCHANGED);

        cv::Mat img2;
        double s=ScaleToD(img1,img2);


        cv::Mat img3 = cv::imread("150.png", CV_LOAD_IMAGE_UNCHANGED);

        cv::Mat img4;
        double s2=ScaleToD(img3,img4);

        cv::Matx33d res;

        res=MapP(img2,img4);



    }


    static void ShowProj(const cv::Mat &frame, cv::Matx33d trans, cv::Size markSize)
    {
        std::vector<cv::Point> vtx(4);
        vtx[0]=cv::Point(0,0);
        vtx[1]=cv::Point(0,markSize.height-1);
        vtx[2]=cv::Point(markSize.width-1,markSize.height-1);
        vtx[3]=cv::Point(markSize.width-1,0);

        for(auto &v1:vtx)
        {
            cv::Vec3d vtxn=trans*cv::Vec3d(v1.x, v1.y, 1);
            v1=cv::Point(vtxn[0]/vtxn[2], vtxn[1]/vtxn[2]);
        }

        cv::polylines(frame, vtx, true, cv::Scalar(0,0,255), 3);

        cv::imshow("ShowProj", frame);
        //        cv::waitKey(-1);

    }

    static cv::Matx33d GetProj(const cv::Mat &mark, const cv::Mat &frame, cv::Rect roi)
    {
        cv::Mat frameroi;
        frame(roi).convertTo(frameroi, mark.type());
        cv::resize(frameroi,frameroi,mark.size());

        cv::Matx33d res=MapP(mark,frameroi);

        cv::Matx33d tpre(((double)roi.width)/mark.cols, 0, roi.x,
                         0, ((double)roi.height)/mark.rows, roi.y,
                         0, 0, 1);

        cv::Matx33d trans=tpre*res;

        return trans;
    }


    static cv::Matx33d GetProj2(const cv::Mat &mark, const cv::Mat &frame, cv::Matx33d transinit)
    {
        cv::Mat frameroi;

        cv::warpPerspective(frame, frameroi, transinit.inv(), mark.size());

        cv::Mat frameroif;

        frameroi.convertTo(frameroif, mark.type());

        cv::Matx33d res=MapP(mark,frameroif);

        cv::Matx33d trans=transinit*res;

        return trans;
    }

    template<typename T>
    static void Expand1D(T &start, T &length, double scale)
    {
        start-=length*(scale-1)/2;
        length*=scale;
    }





    static void test()
    {
        cv::Mat img1 = cv::imread("/home/u/build-untitled-Desktop-Release/67r.png", CV_LOAD_IMAGE_UNCHANGED);

        cv::Mat img2;
        double s=ScaleToD(img1,img2);

        cv::Mat img3 = cv::imread("/home/u/build-untitled-Desktop-Debug/278.png", CV_LOAD_IMAGE_UNCHANGED);

        cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> ocr1 = cv::text::loadOCRHMMClassifierCNN("OCRBeamSearch_CNN_model_data.xml.gz");

        cv::Rect rc[2];

        double p=LocateTwoDigit(img3, 67, ocr1, rc);

        cv::Rect roi=rc[0]|rc[1];

        Expand1D(roi.x, roi.width, 1.3);

        Expand1D(roi.y, roi.height, 1.3);

        std::cout<<"roi="<<roi<<"\n"<<std::flush;

        cv::Matx33d res=GetProj(img2, img3, roi);

        ShowProj(img3, res, img2.size());

        cv::waitKey(1e4);

        cv::destroyAllWindows();

    }


    static bool BestTwo(const std::vector<uchar> &status, const std::vector<float> &err, size_t &index0, size_t &index1)
    {
        bool res=false;

        size_t i=0;
        while(i<status.size())
        {
            if(status[i]>0)
            {
                index0=i;
                i++;

                while(i<status.size())
                {
                    if(status[i]>0)
                    {
                        res=true;

                        index1=i;

                        if(err[index0]>err[index1])
                            std::swap(index0,index1);

                        i++;

                        while(i<status.size())
                        {
                            if(status[i]>0 && err[i]<err[index1])
                            {
                                if(err[index0]<err[i])
                                    index1=i;
                                else
                                {
                                    index1=index0;
                                    index0=i;
                                }

                            }
                            i++;

                        }


                    }
                    i++;
                }

            }
            i++;
        }

        return res;
    }


    static void test2()
    {
        cv::Mat img1 = cv::imread("/home/u/build-untitled-Desktop-Release/67r.png", CV_LOAD_IMAGE_UNCHANGED);

        cv::imshow("img1",img1);

//        cv::Mat img2;
//        double s=ScaleToD(img1,img2);

        Plate plate;

        cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> ocr1 = cv::text::loadOCRHMMClassifierCNN("OCRBeamSearch_CNN_model_data.xml.gz");

        std::string path="/home/u/TV_CAM_设备_20180427_111735.mkv_20180427_115839.avi";

        cv::VideoCapture c(path);
        cv::Mat m;
        cv::namedWindow(path,cv::WINDOW_NORMAL);

        cv::TermCriteria termcrit(cv::TermCriteria::COUNT|cv::TermCriteria::EPS,20,0.03);
        cv::Size subPixWinSize(30,30), winSize(31,31);


        int i=0;

        while(c.read(m))
        {
            std::cout<<i++<<"\n"<<std::flush;
            if(i>278)
                break;
        }


        cv::Rect rc[2];

        double p=LocateTwoDigit(m, plate.number, ocr1, rc);

        cv::Rect roi=rc[0]|rc[1];

        Expand1D(roi.x, roi.width, 1.3);

        Expand1D(roi.y, roi.height, 1.3);

        std::cout<<"roi="<<roi<<"\n"<<std::flush;

        cv::Matx33d res=GetProj(plate.image, m, roi);

//        ShowProj(m, res, img2.size());

//        cv::waitKey(1e5);




        cv::Mat img2r;
        cv::warpPerspective(m, img2r, res.inv(), plate.image.size());



        cv::Mat gray, prevGray;
        std::vector<cv::Point2f> points[2];

        points[0].assign(plate.vertex.begin(),plate.vertex.end());


        cv::cvtColor(img2r, gray, cv::COLOR_BGR2GRAY);

        cv::cornerSubPix(gray, points[0], subPixWinSize, cv::Size(-1,-1), termcrit);


        for(auto &pt:points[0])
        {
            cv::Vec3d vn=res*cv::Vec3d(pt.x,pt.y,1);
            pt.x=vn[0]/vn[2];
            pt.y=vn[1]/vn[2];
        }


        cv::Mat K,D;
        USBCamera::Load(K, "Kf.yml");
        USBCamera::Load(D, "Df.yml");

        double scale=1280/1920.;




        while(c.read(m))
        {
            cv::cvtColor(m, gray, cv::COLOR_BGR2GRAY);

            std::vector<uchar> status;
            std::vector<float> err;
            if(prevGray.empty())
                gray.copyTo(prevGray);
            cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize,
                    3, termcrit, 0, 0.01);

            cv::imwrite("f.png",m);

//            size_t chooseIndex[2];

//            bool found=BestTwo(status,err,chooseIndex[0],chooseIndex[1]);

//            if(found)
            {
//                std::cout<<"found="<<chooseIndex[0]<<"\t"<<points[1][chooseIndex[0]]<<"\n"<<std::flush;
//                std::cout<<"found="<<chooseIndex[1]<<"\t"<<points[1][chooseIndex[1]]<<"\n"<<std::flush;
//                cv::circle( m, points[1][chooseIndex[0]], 3, cv::Scalar(0,255,0), -1, 8);
//                cv::circle( m, points[1][chooseIndex[1]], 3, cv::Scalar(0,255,0), -1, 8);

//                cv::Vec3d xya=plate.LocationF(points[1][chooseIndex[0]]*scale, chooseIndex[0],
//                        points[1][chooseIndex[1]]*scale, chooseIndex[1], K, D, 0);

//                std::cout<<"xya="<<xya<<"\n"<<std::flush;


            }


            cv::Vec3d xya2=plate.LocationF(status, err, points[1], K, D, 0, 10);


            std::cout<<"xya2="<<xya2<<"\n"<<std::flush;


            std::cout<<"status="<<cv::Mat(status)<<"\n"<<std::flush;
            std::cout<<"err="<<cv::Mat(err)<<"\n"<<std::flush;

            for(auto &pt:points[1])
            {
                cv::circle( m, pt, 3, cv::Scalar(0,255,0), -1, 8);
            }



            cv::imshow(path,m);
            int k=cv::waitKey(1000);



            std::cout<<i++<<"\n"<<std::flush;


            std::swap(points[1], points[0]);
            cv::swap(prevGray, gray);
        }

        c.release();
        cv::destroyWindow(path);


    }


};

#endif // DIGITA_HPP

